/** \file    affine.h
  * \brief   Definition of affine transform.
  * \date    2001 - 2003
  * \author  Pavel Agapov, Fedor Chelnokov
  * \version $Header: /home/cscom1r4/cvsroot/cs/include/typelib/affine.h,v 1.3 2007/07/12 20:30:09 cscom1r4 Exp $
  */

#ifndef _affine_h_
#define _affine_h_

#include "types.h"
#include <limits>

namespace types {

// Affine transform is one of the form: Affine(vec{p}) = A*vec{p} + vec{b}, where
//     [ xx xy ]           [ x ]
// A = [ yx yy ], vec{b} = [ y ]
//
template <class T>
struct basic_affine 
{
	T x, y, xx, xy, yx, yy;

	basic_affine() 
		{}
	basic_affine(T _x, T _y, T _xx, T _xy, T _yx, T _yy)
		{ x = _x; y = _y; xx = _xx; xy = _xy; yx = _yx; yy = _yy; }
	basic_affine & identity() 
		{ x = y = 0; xx = yy = 1; xy = yx = 0; 
		  return * this; }

	friend bool operator == (const basic_affine<T> & lhs, const basic_affine<T> & rhs) {
		return lhs.x == rhs.x && lhs.y == rhs.y &&
			lhs.xx == rhs.xx && lhs.xy == rhs.xy &&
			lhs.yx == rhs.yx && lhs.yy == rhs.yy;
	}
	friend bool operator != (const basic_affine<T> & lhs, const basic_affine<T> & rhs) 
		{ return !(lhs == rhs); }

	T det() const
		{ return xx * yy - xy * yx; };
	operator bool() const// true if not degenerate
		{ return det() != 0; };

	///tests whether affine transformation is indeed translation on integer values (dx, dy)
	bool int_translation(int_point * d = 0) const {
		T eps = std::numeric_limits<T>::epsilon() * 10;
		if (fabs(xx - 1) > eps || fabs(xy) > eps || fabs(yx) > eps || fabs(yy - 1) > eps)
			return false;
		int ix = int(x + T(0.5));
		int iy = int(y + T(0.5));
		if (fabs(x - ix) > eps || fabs(y - iy) > eps)
			return false;
		if (d)
			*d = int_point(ix, iy);
		return true;
	}

	basic_affine<T> & translate(T tx, T ty) {
		x += tx;
		y += ty;
		return * this;
	}
	basic_affine<T> & translate(const point<T> & p) 
		{ return translate(p.x, p.y); }
	///makes rigid transformation from rigid+zoom
	void rigid() {
		assert(xx == yy && xy == -yx);
		double s = _hypot(xx, xy);
		xx = yy = (xx / s);
		yx = - (xy = (xy / s));
	}

	point<T> operator ()(T x0, T y0) const {
		return point<T> (
			x + xx * x0 + xy * y0,
			y + yx * x0 + yy * y0
		);
	};
	point<T> operator ()(const point<T> & p) const 
		{ return operator ()(p.x, p.y); }

	// inverse for Affine{A;vec{b}} is Affine{A^{-1};-A^{-1}vec{b}}

	basic_affine<T> inverse() const {
		T d = det();
		assert(d != 0);
		T rd = 1 / d;
		return basic_affine<T> (
			(- yy * x + xy * y) * rd,
			(+ yx * x - xx * y) * rd,
			yy * rd,
			-xy * rd,
			-yx * rd,
			xx * rd
		);
	}

	point<T> inverse(T x0, T y0) const {
		T d = det();
		assert(d != 0);
		return point<T> (
			(+ yy * (x0 - x) - xy * (y0 - y)) / d,
			(- yx * (x0 - x) + xx * (y0 - y)) / d
		);
	};
	point<T> inverse(const point<T> & p) const 
		{ return inverse(p.x, p.y); }

	// product of Affine{A_1;vec{b_1}}*Affine{A_0;vec{b_0}} 
	// is Affine{A_1*A_0; A_1*vec{b_0}+vec{b_1}}

	friend basic_affine<T> operator * (
		const basic_affine<T> & a1, const basic_affine<T> & a0) 
	{
		return basic_affine<T> (
			a1.xx * a0.x + a1.xy * a0.y + a1.x,
			a1.yx * a0.x + a1.yy * a0.y + a1.y,
			a1.xx * a0.xx + a1.xy * a0.yx,
			a1.xx * a0.xy + a1.xy * a0.yy,
			a1.yx * a0.xx + a1.yy * a0.yx,
			a1.yx * a0.xy + a1.yy * a0.yy
		);
	}

	///for rigid and rigid+zoom approximation
	T angle() const 
		{ return atan2(-xy+yx, xx+yy); }
	///for rigid+zoom approximation
	T scale() const 
		{ return 0.5 * hypot(xy-yx, xx+yy); }

	//represents matrix of linear coefficients in the form of product
	//     [c -s]   [1 0]   [k1 0]
	// A = [s  c] * [a 1] * [0 k2], s = sin(phi), c = cos(phi)
	struct decomposition1 {
		T phi, a, k1, k2;
	};
	void decompose(decomposition1 & res) const {
		res.a = (xx*xy + yx*yy) / (xx*yy - xy*yx);
		res.k1 = sqrt((square(xx) + square(yx)) / (1 + square(res.a)));
		res.k2 = sqrt(square(xy) + square(yy));
		res.phi = atan2(-xy, yy);
	}

	//represents matrix of linear coefficients in the form of product
	//         [c' s']   [1 0]   [c' -s']   [c -s]
	// A = k * [s' c'] * [a 1] * [s'  c'] * [s  c], 
	// s' = sin(beta), c' = cos(beta), s = sin(phi), c = cos(phi)
	struct decomposition2 {
		T k, beta, a, phi;
	};
	void decompose(decomposition2 & res) const {
		T Delta = xx*yy - xy*yx;
		T Sigma = square(xx) + square(xy) + square(yx) + square(yy);
		res.k = sqrt(Delta);
		T a2 = Sigma/Delta - 2;
		bool ordinary_skew = (xx + xy - yx - yy) < 0;
		if (a2 <= 0) 
			res.a = 0; //it's not bug workaround, but it may due to round errors
		else {
			if (ordinary_skew)
				res.a = sqrt(a2);
			else
				res.a = -sqrt(a2);
		}
		T p = xx + yy;
		T q = xy - yx;
		T ks = 2*q + p*res.a; //not s, but only proportional to s
		T kc = 2*p - q*res.a;
		res.phi = atan2(-ks, kc);
		T t = xy + yx;
		T u = xx - yy;
		if (ordinary_skew)
			res.beta = 0.5 * atan2(ks*t + kc*u, kc*t - ks*u);
		else
			res.beta = 0.5 * atan2(-(ks*t + kc*u), -(kc*t - ks*u));
	}

	//represents matrix of linear coefficients in the form of product
	//     [c -s]   [k1 0]   [1 0]   
	// A = [s  c] * [0 k2] * [a 1], s = sin(phi), c = cos(phi)
	struct decomposition3 {
		T phi, a, k1, k2;
	};
	void decompose(decomposition3 & res) const {
		T denom = square(xy) + square(yy);
		res.k2 = sqrt(denom);
		res.a = (xx*xy + yx*yy) / denom;
		res.k1 = (xx*yy - xy*yx) / res.k2;
		res.phi = atan2(-xy, yy);
	}
};

typedef basic_affine<double> affine;
typedef basic_affine<float>  affinef;
typedef basic_affine<long double> affinel;

} //namespace types

using namespace types;

#endif //_affine_h_
